#line 1 "D:/Archivio/mikroC/tric-ricotta/tric.c"
#line 1 "d:/archivio/mikroc/tric-ricotta/config.h"
#line 1 "d:/archivio/mikroc/tric-ricotta/sensors.h"



void SENSORS_init();
void SENSORS_read(float *SENSORS_acc, float *SENSORS_gyro);
void SENSORS_process(float *SENSORS_acc, float *SENSORS_gyro, float *SENSORS_angle);
float SENSORS_complementaryFilter(float oldAngle, float newAngle, float newRate);
#line 1 "d:/archivio/mikroc/tric-ricotta/pid.h"



void PID_update(float *RX_angle, float *SENSORS_angle, float *PID_output);
void PID_applyToMotors(float *PID_output, unsigned short *MOTORS_speed);
#line 1 "d:/archivio/mikroc/tric-ricotta/motors.h"



void MOTORS_update(unsigned short *MOTORS_speed);
void MOTORS_esc_startup();
void MOTORS_esc_config();
#line 1 "d:/archivio/mikroc/tric-ricotta/rx.h"



void RX_processValues(unsigned short *RX_interruptTimer, float *RX_values);
void RX_applyThrottleToMotors(float *RX_angle, unsigned short *MOTORS_speed);
#line 1 "d:/archivio/mikroc/tric-ricotta/util.h"



void Soft_UART_Write_Text(char* testo);
unsigned short UTIL_checkBattery();
void UTIL_initCheckBattery();
#line 16 "D:/Archivio/mikroC/tric-ricotta/tric.c"
unsigned short THREAD_counts;
unsigned short THREAD_ready;
unsigned short RX_counts;
unsigned short MOTORS_speed[4];
unsigned short RX_interruptTimer[7];
unsigned short THREAD_emergencyLanding;
float SENSORS_acc[3];
float SENSORS_gyro[3];
float SENSORS_angle[3];
float PID_output[3];
float RX_values[7];
#line 31 "D:/Archivio/mikroC/tric-ricotta/tric.c"
void main() {
#line 43 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 ADCON1 = 0x0F;


 CMCON = 0xCF;


 TRISA = 0b00000001;
 TRISB = 0b00000100;
 TRISC = 0b00000000;


  PORTC.B2  = 1;
  PORTB.B5  = 0;
  PORTB.B3  = 0;
  PORTB.B6  = 0;
  PORTA.B5  = 0;
  PORTA.B4  = 0;
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(1000);
#line 71 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 SENSORS_init();
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(1000);
  PORTC.B2  = 0;
  PORTB.B5  = 1;



 T0CON.T0PS0 = 0;
 T0CON.T0PS1 = 1;
 T0CON.T0PS2 = 1;
 T0CON.PSA = 1;
 T0CON.T0CS = 0;
 T0CON.T08BIT = 0;
 TMR0L = 0;
 TMR0H = 0;
 T0CON.TMR0ON = 0;



 T1CON.T1CKPS1 = 0;
 T1CON.T1CKPS0 = 0;
 TMR1H = 0xD1;
 TMR1L = 0x80;
 PIR1.TMR1IF = 0 ;
 T1CON.TMR1ON = 0;



 INTCON.GIE = 1 ;
 INTCON.PEIE = 1 ;
 PIE1.TMR1IE = 1 ;
 PIR1.TMR1IF = 0 ;
 INTCON.TMR0IE = 0;
 INTCON.TMR0IF = 0;
 INTCON2.INTEDG2 = 1;
 INTCON3.INT2IE = 1;
 INTCON3.INT2IF = 0;



 THREAD_counts = 0;
 THREAD_ready = 0;
 RX_counts=0;
 THREAD_emergencyLanding = 0;
 MOTORS_speed[0] = 55;
 MOTORS_speed[1] = 55;
 MOTORS_speed[2] = 55;
 MOTORS_speed[3] = 55;
#line 128 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 MOTORS_esc_startup();
  PORTB.B5  = 0;
 T1CON.TMR1ON = 1;
#line 145 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 while (1) {
 if(THREAD_ready){
 switch(THREAD_counts){
 case 1:
  PORTC.B2  = ! PORTC.B2 ;
 break;

 case 11:

 break;
 case 13:

 break;

 case 20:

  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
  PORTC.B7  = 0;



 case 0:
#line 179 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 2:
#line 184 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 4:
#line 189 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 6:
#line 194 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 8:
#line 199 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 10:
#line 204 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 12:
#line 209 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 14:
#line 214 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 case 16:
 case 18:





 RX_applyThrottleToMotors(&RX_values, &MOTORS_speed);
 break;

 case 21:
 MOTORS_update(&MOTORS_speed);
 THREAD_counts = 0;
 break;
 }
 THREAD_ready = 0;
 }
 }
}

void interrupt() {


 if(PIR1.TMR1IF) {
 TMR1H = 0xD1;
 TMR1L = 0x80;
 PIR1.TMR1IF = 0;
 THREAD_counts++;
 THREAD_ready = 1;
 }
#line 258 "D:/Archivio/mikroC/tric-ricotta/tric.c"
 }
